
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       pid.h
  * @author     baiyang
  * @date       2021-8-8
  ******************************************************************************
  */

#pragma once

#ifdef __cplusplus
extern "C"{
#endif

/*----------------------------------include-----------------------------------*/
#include <stdbool.h>
#include <stdint.h>
#include <common/gp_math/gp_mathlib.h>
/*-----------------------------------macro------------------------------------*/
#define PID_TFILT_HZ_DEFAULT  0.0f   // default input filter frequency
#define PID_EFILT_HZ_DEFAULT  0.0f   // default input filter frequency
#define PID_DFILT_HZ_DEFAULT  20.0f  // default input filter frequency
#define PID_RESET_TC          0.16f  // Time constant for integrator reset decay to zero
/*----------------------------------typedef-----------------------------------*/
typedef struct
{
    float _kp;
    float _ki;
    float _kd;
    float _kff;
    float _kimax;
    float _filt_t_hz;
    float _filt_e_hz;
    float _filt_d_hz;
    float _slew_rate_max;

    bool _flags_reset_filter;

    // internal variables
    float dt;                // timestep in seconds
    float integrator;        // integrator value
    float target;            // target value to enable filtering
    float error;             // error value to enable filtering
    float derivative;        // derivative value to enable filtering

    uint16_t reset_counter;  // loop counter for reset decay
    uint64_t reset_last_update; //time in microseconds of last update to reset_I
}PID_ctrl;

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void pid_ctrl_ctor(PID_ctrl * controler, float kp, float ki, float kd, float kff, float kimax, float filt_t_hz, float filt_e_hz, float filt_d_hz, float initial_dt);

// set_dt - set time step in seconds
static inline void pid_ctrl_set_dt(PID_ctrl * controler, float initial_dt) { controler->dt = initial_dt; }

//  update_all - set target and measured inputs to PID controller and calculate outputs
//  target and error are filtered
//  the derivative is then calculated and filtered
//  the integral is then updated based on the setting of the limit flag
float pid_ctrl_update_all(PID_ctrl * controler, float target, float measurement, bool limit);

//  update_error - set error input to PID controller and calculate outputs
//  target is set to zero and error is set and filtered
//  the derivative then is calculated and filtered
//  the integral is then updated based on the setting of the limit flag
//  Target and Measured must be set manually for logging purposes.
// todo: remove function when it is no longer used.
float pid_ctrl_update_error(PID_ctrl * controler, float error, bool limit);

//  update_i - update the integral
//  if the limit flag is set the integral is only allowed to shrink
void pid_ctrl_update_i(PID_ctrl * controler, bool limit);

// get_pid - get results from pid controller
static inline float pid_ctrl_get_pid(PID_ctrl * controler) { return controler->error * controler->_kp + controler->integrator + controler->_kd * controler->derivative; }
static inline float pid_ctrl_get_pi(PID_ctrl * controler) { return controler->error * controler->_kp + controler->integrator; }
static inline float pid_ctrl_get_p(PID_ctrl * controler) { return controler->error * controler->_kp; }
static inline float pid_ctrl_get_i(PID_ctrl * controler) { return controler->integrator; }
static inline float pid_ctrl_get_d(PID_ctrl * controler) { return controler->_kd * controler->derivative; }
static inline float pid_ctrl_get_ff(PID_ctrl * controler) { return controler->target * controler->_kff; }

// todo: remove function when it is no longer used.
static inline float pid_ctrl_get_ff2(PID_ctrl * controler, float target) { return target * controler->_kff; }

// reset_I - reset the integrator
static inline void pid_ctrl_reset_i(PID_ctrl * controler) { controler->integrator = 0; }

// reset_I_smoothly - reset the integrator
void pid_ctrl_reset_i_smoothly(PID_ctrl * controler);

// reset_filter - input filter will be reset to the next value provided to set_input()
static inline void pid_ctrl_reset_filter(PID_ctrl * controler) { controler->_flags_reset_filter = true; }

// set accessors
static inline void pid_ctrl_set_kp(PID_ctrl * controler, float kp) { controler->_kp = kp; }
static inline void pid_ctrl_set_ki(PID_ctrl * controler, float ki) { controler->_ki = ki; }
static inline void pid_ctrl_set_kd(PID_ctrl * controler, float kd) { controler->_kd = kd; }
static inline void pid_ctrl_set_kff(PID_ctrl * controler, float kff) { controler->_kff = kff; }
static inline void pid_ctrl_set_kimax(PID_ctrl * controler, float kimax) { controler->_kimax = kimax; }
static inline void pid_ctrl_set_filt_t_hz(PID_ctrl * controler, float filt_t_hz) { controler->_filt_t_hz = fabsf(filt_t_hz); }
static inline void pid_ctrl_set_filt_e_hz(PID_ctrl * controler, float filt_e_hz) { controler->_filt_e_hz = fabsf(filt_e_hz); }
static inline void pid_ctrl_set_filt_d_hz(PID_ctrl * controler, float filt_d_hz) { controler->_filt_d_hz = fabsf(filt_d_hz); }

static inline float pid_ctrl_get_kp(PID_ctrl * controler) { return controler->_kp; }
static inline float pid_ctrl_get_ki(PID_ctrl * controler) { return controler->_ki; }
static inline float pid_ctrl_get_kd(PID_ctrl * controler) { return controler->_kd; }
static inline float pid_ctrl_get_kff(PID_ctrl * controler) { return controler->_kff; }
static inline float pid_ctrl_get_kimax(PID_ctrl * controler) { return controler->_kimax; }
static inline float pid_ctrl_get_filt_t_hz(PID_ctrl * controler) { return controler->_filt_t_hz; }
static inline float pid_ctrl_get_filt_e_hz(PID_ctrl * controler) { return controler->_filt_e_hz; }
static inline float pid_ctrl_get_filt_d_hz(PID_ctrl * controler) { return controler->_filt_d_hz; }
float pid_ctrl_get_filt_alpha(PID_ctrl * controler, float filt_hz);
float pid_ctrl_get_filt_t_alpha(PID_ctrl * controler);
float pid_ctrl_get_filt_e_alpha(PID_ctrl * controler);
float pid_ctrl_get_filt_d_alpha(PID_ctrl * controler);

// integrator setting functions
void pid_ctrl_set_integrator3(PID_ctrl * controler, float target, float measurement, float i);
void pid_ctrl_set_integrator2(PID_ctrl * controler, float error, float i);
void pid_ctrl_set_integrator1(PID_ctrl * controler, float i);

/*------------------------------------test------------------------------------*/

#ifdef __cplusplus
}
#endif



